/**  
 * Copyright (c) 2010 University of Pennsylvania.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 */
 
void init_mheapman()
{
  memset(heapman_buffer, 0, sizeof(heapman_buffer));
  memset(heapman_used, 0, sizeof(heapman_used));
  memset(heapman_blockend, 0, sizeof(heapman_blockend));
  heapman_firstfreeblock = 0; heapman_usage = 0; heapman_maxusage = 0;
}

uint16_t heapman_available()
{
  return HEAPMAN_BUFFERSIZE-heapman_usage;
}

uint8_t heapman_findfreeblock(uint16_t size, uint16_t *block_start, uint16_t *block_end)
{
  uint16_t size_blocks = DIV_CEIL(size, HEAPMAN_BLOCKSIZE);

  if (size == 0)
    return 0;

  *block_start = heapman_firstfreeblock;
  *block_end = 0;
  for (;;)
  {
    while (*block_start < HEAPMAN_BLOCKCOUNT && BV_GET(heapman_used, *block_start))
      (*block_start)++;

    if (*block_start >= HEAPMAN_BLOCKCOUNT)
      return 0;

    *block_end = *block_start;
    while (*block_end < HEAPMAN_BLOCKCOUNT && BV_GET(heapman_used, *block_end) == 0 &&
      *block_end-*block_start < size_blocks-1)
      (*block_end)++;

    if (*block_end < HEAPMAN_BLOCKCOUNT && BV_GET(heapman_used, *block_end) == 0 &&
      *block_end-*block_start >= size_blocks-1)
      return 1;

    *block_start = (*block_end)+1;
  }
}

uint8_t heapman_findblock(void *p, uint16_t *block_start, uint16_t *block_end)
{
  if (p == NULL || (uint8_t *) p < heapman_buffer ||
    (uint8_t *) p >= heapman_buffer+sizeof(heapman_buffer))
    return 0;

  *block_start = (((uint8_t *) p)-heapman_buffer)/HEAPMAN_BLOCKSIZE;
  if ((*block_start) > 0 && BV_GET(heapman_used, (*block_start)-1) &&
    BV_GET(heapman_blockend, (*block_start)-1) == 0)
    return 0;                // in the middle of a block, previous block is not free or end

  *block_end = *block_start;
  while (*block_end < HEAPMAN_BLOCKCOUNT && BV_GET(heapman_used, *block_end) &&
    BV_GET(heapman_blockend, *block_end) == 0)
    (*block_end)++;

  return (*block_end < HEAPMAN_BLOCKCOUNT && BV_GET(heapman_used, *block_end) &&
    BV_GET(heapman_blockend, *block_end));
}

void heapman_setblock(uint16_t block_start, uint16_t block_end, uint8_t bit)
{
  int i;

  for (i = block_start; i <= block_end; i++)
    BV_SET(heapman_used, i, bit);
  BV_SET(heapman_blockend, block_end, bit);

  if (bit)
  {
    if (heapman_firstfreeblock == block_start)
      heapman_firstfreeblock = block_end+1;

    heapman_usage += HEAPMAN_BLOCKSIZE*(block_end-block_start+1);
    if (heapman_usage > heapman_maxusage)
      heapman_maxusage = heapman_usage;
  } else {
    if (heapman_firstfreeblock > block_start)
      heapman_firstfreeblock = block_start;

    heapman_usage -= HEAPMAN_BLOCKSIZE*(block_end-block_start+1);
  }
}

void *heapman_malloc(uint16_t size)
{
  uint8_t *p;
  uint16_t block_start, block_end;

  if (heapman_findfreeblock(size, &block_start, &block_end))
  {
    heapman_setblock(block_start, block_end, 1);
    p = &heapman_buffer[HEAPMAN_BLOCKSIZE*block_start];
    memset(p, 0, HEAPMAN_BLOCKSIZE*(block_end-block_start+1));
    return p;
  } else return NULL;
}

void heapman_free(void *p)
{
  uint16_t block_start, block_end;
  
  if (heapman_findblock(p, &block_start, &block_end))
    heapman_setblock(block_start, block_end, 0);
}

void *heapman_calloc(uint16_t size, uint16_t blocks)
{
  return heapman_malloc(size*blocks);
}

void *heapman_realloc(void *p, uint16_t size)
{
  int i;
  uint16_t oldblock_start, oldblock_end, newblock_start, newblock_end, size_blocks;

  if (p == NULL)
    return heapman_malloc(size);

  if (size == 0)
  {
    heapman_free(p);
    return NULL;
  }

  if (heapman_findblock(p, &oldblock_start, &oldblock_end))
  {
    heapman_setblock(oldblock_start, oldblock_end, 0);
    if (heapman_findfreeblock(size, &newblock_start, &newblock_end) == 0)
    {                           // Cannot allocate enough memory
      heapman_setblock(oldblock_start, oldblock_end, 1);
      return NULL;
    }

    size_blocks = MIN(oldblock_end-oldblock_start+1, newblock_end-newblock_start+1);
    if (newblock_start < oldblock_start)
      for (i = 0; i < size_blocks; i++)
        memcpy(&heapman_buffer[HEAPMAN_BLOCKSIZE*(newblock_start+i)],
          &heapman_buffer[HEAPMAN_BLOCKSIZE*(oldblock_start+i)], HEAPMAN_BLOCKSIZE);
    else if (newblock_start > oldblock_start)
      for (i = size_blocks-1; i >= 0; i--)
        memcpy(&heapman_buffer[HEAPMAN_BLOCKSIZE*(newblock_start+i)],
          &heapman_buffer[HEAPMAN_BLOCKSIZE*(oldblock_start+i)], HEAPMAN_BLOCKSIZE);

    heapman_setblock(newblock_start, newblock_end, 1);
    return &heapman_buffer[HEAPMAN_BLOCKSIZE*newblock_start];
  } else
    return NULL;
}

#ifdef DEBUG_MEM
const char *format_fnname(const char *fn_name)
{
  const char *p = strstr(fn_name, "$");
  return p ? p+1 : fn_name;
}
#endif

#ifdef DEBUG_MEM
void *__mmalloc(uint16_t size, const char *fn_name, int line)
#else
void *__mmalloc(uint16_t size)
#endif
{
  void *p;

#ifdef DEBUG_MEM
  DBGOUT("%s, %d: Allocating...", format_fnname(fn_name), line);
#endif

#ifdef USE_HEAPMAN
  p = heapman_malloc(size);
#else
  p = malloc(size);
#endif

  if (p == NULL)
  {
#ifdef TOSSIM
    DBGERR("Cannot allocate %lu bytes!", size);
    log_dropcmd(1, &TOS_NODE_ID, NULL, 0, 8);
#endif
    return NULL;
  }

#ifdef DEBUG_MEM
  DBGOUT("%s, %d: Allocated pointer %d, requested: %lu, mem_usage: %lu, max_usage: %lu",
    format_fnname(fn_name), line, (uint8_t *) p-heapman_buffer, size, heapman_usage, heapman_maxusage);
#endif

  return p;
}

#ifdef DEBUG_MEM
void *__mcalloc(uint16_t size, uint16_t block_count, const char *fn_name, int line)
#else
void *__mcalloc(uint16_t size, uint16_t block_count)
#endif
{
  void *p;

#ifdef DEBUG_MEM
  DBGOUT("%s, %d: Allocating...", format_fnname(fn_name), line);
#endif

#ifdef USE_HEAPMAN
  p = heapman_calloc(size, block_count);
#else
  p = calloc(size, block_count);
#endif

  if (p == NULL)
  {
#ifdef TOSSIM
    DBGERR("Cannot allocate %lu bytes!", size*block_count);
    log_dropcmd(1, &TOS_NODE_ID, NULL, 0, 8);
#endif
    return NULL;
  }

#ifdef DEBUG_MEM
  DBGOUT("%s, %d: Allocated pointer %d, requested: %lu, mem_usage: %lu, max_usage: %lu",
    format_fnname(fn_name), line, (uint8_t *) p-heapman_buffer, size*block_count, heapman_usage,
    heapman_maxusage);
#endif

  return p;
}

#ifdef DEBUG_MEM
void *__mrealloc(void *p, uint16_t size, const char *fn_name, int line)
#else
void *__mrealloc(void *p, uint16_t size)
#endif
{
  void *p1;

#ifdef DEBUG_MEM
  DBGOUT("%s, %d: Rellocating...", format_fnname(fn_name), line);
#endif

#ifdef USE_HEAPMAN
  p1 = heapman_realloc(p, size);
#else
  p1 = realloc(p, size);
#endif

  if (size > 0 && p1 == NULL)
  {
#ifdef TOSSIM
    DBGERR("Cannot reallocate %lu bytes!", size);
    log_dropcmd(1, &TOS_NODE_ID, NULL, 0, 8);
#endif    
    return NULL;
  }

#ifdef DEBUG_MEM
  DBGOUT("%s, %d: Reallocated pointer %d to %d, requested: %lu, mem_usage: %lu, max_usage: %lu",
    format_fnname(fn_name), line, (uint8_t *) p-heapman_buffer, (uint8_t *) p1-heapman_buffer,
    size, heapman_usage, heapman_maxusage);
#endif

  return p1;
}

#ifdef DEBUG_MEM
void __mfree(void *p, const char *fn_name, int line)
#else
void __mfree(void *p)
#endif
{
#ifndef DISABLE_FREE
#ifdef DEBUG_MEM
  DBGOUT("%s, %d: Freeing...", format_fnname(fn_name), line);
#endif

#ifdef USE_HEAPMAN
  heapman_free(p);
#else
  free(p);
#endif

#ifdef DEBUG_MEM
  DBGOUT("%s, %d: Freed pointer %d, mem_usage: %lu, max_usage: %lu", format_fnname(fn_name),
    line, (uint8_t *) p-heapman_buffer, heapman_usage, heapman_maxusage);
#endif
#endif
}

